#include "common_def.h"
#include "soc_osal.h"
#include "app_init.h"
#include "securec.h"
#include "cJSON.h"

#include <los_sem.h>

#include "sle_low_latency.h"
#include "sle_server.h"
#include "sle_server_adv.h"
#include "sle_device_discovery.h"
#include "sle_errcode.h"
#include "timer.h"

#include "bsp_led.h"
#include "bsp_motor.h"
#include "bsp_uart.h"
#include "PID.h"
#include "bsp_ldr.h"
#include <math.h>
#define CODE_LOG "[ShipSystem ControlNode]"

#define SLE_SERVER_MSG_QUEUE_LEN 5
#define SLE_SERVER_MSG_QUEUE_MAX_SIZE 32
#define SLE_SERVER_QUEUE_DELAY 0xFFFFFFFF
#define SLE_SERVER_BUFF_MAX_SIZE 800

#define MOTOR_EVENT_CONTROL_UPDATA 0x0001 // 控制更新事件
#define MOTOR_EVENT_ALL 0x0001            // 所有系统事件
void communication_uart_rx_callback(const void *buffer, uint16_t length, bool error);
void PID_out(uint16_t left, uint16_t right);
static uint8_t g_uart_gui_rx_buff[512] = {0};
static uart_buffer_config_t g_uart_gui_buffer_config = {
    .rx_buffer = g_uart_gui_rx_buff,
    .rx_buffer_size = 512,
};
Attitude ship_attitude = {0};
typedef enum
{
    MOTOR_STOP = 0,
    MOTOR_ADVANCE,
    MOTOR_BACK,
    MOTOR_LEFT,
    MOTOR_RIGHT,
} MotorControl;
uint32_t electric_quantity;
int Motor_speed[2];    // 电机速度
uint16_t target_speed; // 目标速度
struct
{
    osal_event MotorEvent; // 电机事件

    MotorControl motor_control; // 电机控制
    uint8_t motor_l_speed;      // 左电机速度   范围 0-100
    uint8_t motor_r_speed;      // 右电机速度   范围 0-100
} g_motor_control = {0};
PID_t Motor1_speed = {
    .Kp = 0.07,
    .Ki = 0.12,
    .Kd = 0.08,
    .OutMax = 100,
    .OutMin = -100,
};
PID_t Motor2_speed = {
    .Kp = 0.07,
    .Ki = 0.12,
    .Kd = 0.08,
    .OutMax = 100,
    .OutMin = -100,
};
PID_t Motor_Angle = {
    .Kp = 5,
    .Ki = 0,
    .Kd = 0.4,
    .OutMax = 40,
    .OutMin = -40,
};
static uint8_t g_motor_l_speed = 80; // 电机速度 取值范围 0-100

// 任务句柄
osal_task *g_TaskHandle_LedBlinky = NULL; // LED闪烁任务

// 定义SLE设备地址 FF + "ship" + Node_ID（0x01）
uint8_t g_sle_device_addr[SLE_ADDR_LEN] = {0xFF, 0x73, 0x68, 0x69, 0x70, 0x01};
// SLE状态 0：未连接 1：连接成功
static uint8_t g_sle_status = 0;

static void ssaps_server_read_request_cbk(uint8_t server_id, uint16_t conn_id, ssaps_req_read_cb_t *read_cb_para, errcode_t status)
{
    osal_printk("%s ssaps read request cbk callback server_id:%x, conn_id:%x, handle:%x, status:%x\r\n", CODE_LOG, server_id, conn_id, read_cb_para->handle, status);
}
static void ssaps_server_write_request_cbk(uint8_t server_id, uint16_t conn_id, ssaps_req_write_cb_t *write_cb_para, errcode_t status)
{
    osal_printk("%s ssaps write request callback cbk server_id:%x, conn_id:%x, handle:%x, status:%x\r\n", CODE_LOG, server_id, conn_id, write_cb_para->handle, status);
    if ((write_cb_para->length > 0) && write_cb_para->value)
    {
        osal_printk("sle uart recived data : %s\r\n", write_cb_para->value);

        // 解析JSON数据
        cJSON *cJson = cJSON_Parse((const char *)write_cb_para->value);
        if (cJson != NULL)
        {
            // 获取数据类型
            cJSON *cJson_type = cJSON_GetObjectItem(cJson, "type");
            if (cJson_type != NULL && cJSON_IsString(cJson_type))
            {
                osal_printk("sle uart recived data type : %s\r\n", cJson_type->valuestring);

                // 判断数据类型
                if (osal_strcmp(cJson_type->valuestring, "motor_control") == 0) // 控制电机
                {
                    cJSON *cJson_mode = cJSON_GetObjectItem(cJson, "mode");
                    cJSON *cJson_speed = cJSON_GetObjectItem(cJson, "speed");
                    if (cJson_mode != NULL && cJSON_IsString(cJson_mode) && cJson_speed != NULL && cJSON_IsNumber(cJson_speed) && cJson_speed->valueint >= 0 && cJson_speed->valueint <= 100)
                    {
                        osal_printk("sle uart recived data mode : %s, speed : %d\r\n", cJson_mode->valuestring, cJson_speed->valueint);

                        // 控制电机参数
                        if (osal_strcmp(cJson_mode->valuestring, "advance") == 0) // 前进
                        {
                            g_motor_control.motor_control = MOTOR_ADVANCE;
                            g_motor_control.motor_l_speed = cJson_speed->valueint;
                            g_motor_control.motor_r_speed = cJson_speed->valueint;
                        }
                        else if (osal_strcmp(cJson_mode->valuestring, "back") == 0) // 后退
                        {
                            g_motor_control.motor_control = MOTOR_BACK;
                            g_motor_control.motor_l_speed = cJson_speed->valueint;
                            g_motor_control.motor_r_speed = cJson_speed->valueint;
                        }
                        else if (osal_strcmp(cJson_mode->valuestring, "left") == 0) // 左转
                        {
                            g_motor_control.motor_control = MOTOR_LEFT;
                            g_motor_control.motor_l_speed = cJson_speed->valueint;
                            g_motor_control.motor_r_speed = cJson_speed->valueint;
                        }
                        else if (osal_strcmp(cJson_mode->valuestring, "right") == 0) // 右转
                        {
                            g_motor_control.motor_control = MOTOR_RIGHT;
                            g_motor_control.motor_l_speed = cJson_speed->valueint;
                            g_motor_control.motor_r_speed = cJson_speed->valueint;
                        }
                        else if (osal_strcmp(cJson_mode->valuestring, "stop") == 0) // 停止
                        {
                            g_motor_control.motor_control = MOTOR_STOP;
                            g_motor_control.motor_l_speed = 0;
                            g_motor_control.motor_r_speed = 0;
                        }

                        // 发送电机控制更新事件
                        osal_event_write(&g_motor_control.MotorEvent, MOTOR_EVENT_CONTROL_UPDATA);
                    }
                }
            }

            // 释放JSON对象
            cJSON_Delete(cJson);
        }
        else
        {
            osal_printk("%s sle uart recived data parse fail\r\n", CODE_LOG);
        }
    }
}

/**
 * @brief  SLE服务器控制消息回调函数。
 * @param  buffer_addr       控制消息。
 * @param  buffer_size       控制消息长度。
 * @retval 无
 */
static void sle_server_write_msgqueue(uint8_t *buffer_addr, uint16_t buffer_size)
{
    // osal_printk("%s sle_server_write_msgqueue. buffer_size:%d, buffer:%s\n", CODE_LOG, buffer_size, buffer_addr);

    if (memcmp(buffer_addr, "sle_connect", buffer_size) == 0)
    {
        osal_printk("%s sle_connect\r\n", CODE_LOG);

        // SLE连接成功，关闭设备公开
        g_sle_status = 1;

        // 停止LED闪烁
        osal_kthread_suspend(g_TaskHandle_LedBlinky);
        bsp_led_set_state(LED1, LED_ON); // 开启LED1
    }
    else if (memcmp(buffer_addr, "sle_disconnect", buffer_size) == 0)
    {
        osal_printk("%s sle_disconnect\r\n", CODE_LOG);

        // SLE断开连接，开启设备公开
        g_sle_status = 0;

        // 开启LED闪烁
        osal_kthread_resume(g_TaskHandle_LedBlinky);

        // 客户端断开连接，重新开启设备公开
        errcode_t ret = sle_start_announce(1);
        if (ret != ERRCODE_SLE_SUCCESS)
        {
            osal_printk("%s sle_connect_state_changed_cbk, sle_start_announce fail :%02x\r\n", CODE_LOG, ret);
        }
    }
}

/**
 * @brief  初始化uuid server。
 * @retval ERRCODE_SUCC 成功。
 * @retval Other        失败，参考 @ref errcode_t 。
 */
errcode_t sle_server_init(void)
{
    errcode_t ret;

    // 注册SLE控制消息回调函数
    sle_server_register_msg(sle_server_write_msgqueue);

    // 使能SLE
    ret = enable_sle();
    if (ret != ERRCODE_SUCC)
    {
        osal_printk("%s sle enbale fail!\n", CODE_LOG);

        return ret;
    }

    // 注册SLE设备发现回调函数
    ret = sle_announce_register_cbks();
    if (ret != ERRCODE_SLE_SUCCESS)
    {
        osal_printk("%s sle_server_init, sle_announce_register_cbks fail: %x\n", CODE_LOG, ret);

        return ret;
    }

    // 注册SLE连接管理回调函数
    ret = sle_conn_register_cbks();
    if (ret != ERRCODE_SLE_SUCCESS)
    {
        osal_printk("%s sle_server_init, sle_conn_register_cbks fail: %x\n", CODE_LOG, ret);

        return ret;
    }

    // 注册SLE SSAPS回调函数
    ret = sle_ssaps_register_cbks(ssaps_server_read_request_cbk, ssaps_server_write_request_cbk);
    if (ret != ERRCODE_SLE_SUCCESS)
    {
        osal_printk("%s sle_server_init, sle_ssaps_register_cbks fail :%x\r\n", CODE_LOG, ret);
        return ret;
    }

    // 初始化SLE服务端
    ret = sle_server_add();
    if (ret != ERRCODE_SLE_SUCCESS)
    {
        osal_printk("%s sle_server_init, sle_server_add fail :%x\r\n", CODE_LOG, ret);
        return ret;
    }

    // 初始化SLE服务端设备公开功能
    ret = sle_server_adv_init(CONFIG_SLE_SERVER_NAME, "MOTOR", g_sle_device_addr, SLE_ADDR_LEN);
    if (ret != ERRCODE_SLE_SUCCESS)
    {
        osal_printk("%s sle_server_init, sle_server_adv_init fail :%x\r\n", CODE_LOG, ret);
        return ret;
    }

    osal_printk("%s sle init ok\r\n", CODE_LOG);

    return ERRCODE_SLE_SUCCESS;
}

/**
 * @brief  创建Json消息数据。
 * @param  [in] msg_data_type 消息数据类型。
 * @param  [in] msg_data_unit 消息数据单位。
 * @param  [in] msg_data_value 消息数据值。
 * @retval 成功， 返回Json消息数据字符串。该字符串使用完成需要使用 free() 释放内存。
 * @retval 失败， 返回 NULL。
 */
char *cJSON_MessageCreat(char *speed, char *angle, char *electricity)
{
    // 创建JSON对象
    cJSON *root = cJSON_CreateObject();
    if (root == NULL)
    {
        osal_printk("%s cJSON_CreateObject fail\r\n", CODE_LOG);

        return NULL;
    }
    // 添加type字段
    cJSON_AddStringToObject(root, "equipment", "control");

    // 添加type字段
    cJSON_AddStringToObject(root, "speed", speed);

    // 添加value字段

    cJSON_AddStringToObject(root, "angle", angle);

    // 添加unit字段
    cJSON_AddStringToObject(root, "electricity", electricity);

    // 将JSON对象转换为字符串
    char *json_string = cJSON_Print(root);

    // 释放JSON对象，并返回字符串
    cJSON_Delete(root);
    return json_string;
}
timer_handle_t timer_handle3 = {0}; // 定时器句柄

// led任务
static void *ShipSystem_LedBlinky_task(const char *arg)
{
    unused(arg);

    bsp_led_init(LED1, CONFIG_LED1_PIN);

    while (1)
    {
        bsp_led_toggle(LED1);
        osal_msleep(500);
    }

    return NULL;
}
// Timer配置
timer_handle_t timer_handle = {0};  // 定时器句柄
timer_handle_t timer_handle2 = {0}; // 定时器句柄
// 定时器中断
int left, right;

void timer_timeout_callback(uintptr_t data)
{

    left = Motor_speed[0];
    right = Motor_speed[1];
    Motor_speed[0] = Motor_speed[1] = 0;

    // osal_printk("%d,%d", left, right);

    if (Motor1_speed.Target || Motor2_speed.Target)
    {
        PID_out(left, right);
    }
    else
    {
        Motor1_speed.ErrorInt = Motor2_speed.ErrorInt = 0;
        bsp_motor_stop();
        Motor2_speed.Out = Motor1_speed.Out = 0;
    }
    osal_printk("%d,%d,%d,%d,%d,%d,%d,%d,%d\n", left, (int16_t)Motor1_speed.Target, (int16_t)Motor1_speed.Out, right, (int16_t)Motor2_speed.Target, (int16_t)Motor2_speed.Out, (int16_t)ship_attitude.Jz, (int16_t)Motor_Angle.Target, (int16_t)Motor_Angle.Out);

    uapi_timer_start(timer_handle, 50000, timer_timeout_callback, 0);
}
volatile uint32_t Angle = 0;
volatile uint32_t temp = 0;
volatile bool intdext = 0;
void timer_timeout_callback2(uintptr_t data)
{
    if (Motor1_speed.Target || Motor2_speed.Target)
    {
        if ((temp < Angle) && (intdext == 0))
        {
            uapi_gpio_set_val(GPIO_05, 1);
            intdext = 1;
        }
        else if ((intdext == 1) && (temp > Angle))
        {
            uapi_gpio_set_val(GPIO_05, 0);
            intdext = 0;
        }

        temp = (temp + 1) % 200;
    }
    uapi_timer_start(timer_handle2, 100, timer_timeout_callback2, 0);
}
void dianji1(pin_t pin, uintptr_t param)
{
    Motor_speed[0]++;
}
void dianji2(pin_t pin, uintptr_t param)
{
    Motor_speed[1]++;
}
uint8_t direction;
static void *ShipSystem_ControlNode_task(const char *arg)
{
    unused(arg);

    // 创建系统事件
    if (osal_event_init(&g_motor_control.MotorEvent) != OSAL_SUCCESS)
    {
        osal_printk("ERROR:%s Init MotorEvent failed\n", CODE_LOG);
        return NULL;
    }

    // 初始化设备
    bsp_motor_init();
    // bsp_motor_advance(50);

    // 开启LED闪烁任务
    osal_kthread_lock();
    g_TaskHandle_LedBlinky = osal_kthread_create((osal_kthread_handler)ShipSystem_LedBlinky_task, 0, "ShipSystem_LedBlinky_task", 0x200);
    if (g_TaskHandle_LedBlinky != NULL)
    {
        osal_kthread_set_priority(g_TaskHandle_LedBlinky, 32);
    }
    osal_kthread_unlock();

    // 初始化SLE服务
    sle_server_init();

    bsp_uart_init(UART_BUS_2, 115200, &g_uart_gui_buffer_config);
    bsp_uart_register_rx(UART_BUS_2, communication_uart_rx_callback);

    uapi_timer_init();

    // pid任务
    uapi_timer_adapter(TIMER_INDEX_1, TIMER_1_IRQN, 3);
    uapi_timer_create(TIMER_INDEX_1, &timer_handle);
    uapi_timer_start(timer_handle, 50000, timer_timeout_callback, 0);

    uapi_pin_set_mode(GPIO_05, PIN_MODE_4);
    uapi_gpio_set_dir(GPIO_05, GPIO_DIRECTION_OUTPUT);

    uapi_timer_adapter(TIMER_INDEX_2, TIMER_2_IRQN, 1);
    uapi_timer_create(TIMER_INDEX_2, &timer_handle2);
    uapi_timer_start(timer_handle2, 100, timer_timeout_callback2, 0);

    encoder_init(GPIO_11, GPIO_12, dianji1);
    encoder_init(GPIO_13, GPIO_14, dianji2);

    while (1)
    {

        uint32_t event = osal_event_read(&g_motor_control.MotorEvent, MOTOR_EVENT_ALL, OSAL_WAIT_FOREVER, OSAL_WAITMODE_OR);
        if (event & MOTOR_EVENT_CONTROL_UPDATA) // 电机控制更新事件
        {
            // osal_printk("%s MOTOR_EVENT_CONTROL_UPDATA\n", CODE_LOG);

            // 根据电机控制变量执行相应的操作
            Motor_Angle.OutMax = g_motor_control.motor_l_speed;
            Motor_Angle.OutMin = -g_motor_control.motor_l_speed;

            switch (g_motor_control.motor_control)
            {
            case MOTOR_STOP: // 停止电机
                osal_printk("%s Motor stop\n", CODE_LOG);
                Motor2_speed.Target = Motor1_speed.Target = 0;
                direction = 0;
                break;

            case MOTOR_ADVANCE: // 电机前进

                Motor1_speed.Target = Motor2_speed.Target = target_speed = g_motor_control.motor_l_speed * 2.9;
                Motor_Angle.Target = ship_attitude.Jz;
                break;

            case MOTOR_BACK: // 电机后退
                direction = 1;
                Motor1_speed.Target = Motor2_speed.Target = target_speed = g_motor_control.motor_l_speed * 2.8;
                Motor_Angle.Target = ship_attitude.Jz;
                break;

            case MOTOR_LEFT: // 电机左转
                Motor1_speed.Target = Motor2_speed.Target = target_speed = g_motor_control.motor_l_speed * 2.9;
                Motor_Angle.Target = fmod(ship_attitude.Jz + 90.0, 360.0);
                direction = 2;
                break;

            case MOTOR_RIGHT: // 电机右转
                Motor1_speed.Target = Motor2_speed.Target = target_speed = g_motor_control.motor_l_speed * 2.9;
                Motor_Angle.Target = fmod(ship_attitude.Jz + 270.0, 360.0);
                direction = 3;
                break;

            default:
                break;
            }
        }

        // 清除系统数据更新事件
        osal_event_clear(&g_motor_control.MotorEvent, event);
    }

    // 销毁系统数据更新事件
    osal_event_destroy(&g_motor_control.MotorEvent);

    return NULL;
}
static void *Voltage_acquisition_interruption(const char *arg) // 采集电量任务
{
    bsp_ldr_init(ADC_CHANNEL_2);
    while (1)
    {
        bsp_ldr_get_value(&electric_quantity);
        electric_quantity = (uint32_t)(((electric_quantity - 820.0) / 306.0) * 100.0);
        osal_msleep(10000);
    }
    return NULL;
}
static void *Control_information_upload(const char *arg) // 采集电量任务
{
    char data[3][10];
    char *ShowBuf = NULL;
    while (1)
    {
        if (g_sle_status)
        {
            sprintf(data[0], "%d", (left + right) / 2);
            sprintf(data[1], "%d", (int16_t)ship_attitude.Jz);
            sprintf(data[2], "%d", electric_quantity);
            ShowBuf = cJSON_MessageCreat(data[0], data[1], data[2]);
            if (ShowBuf != NULL)
            {
                // osal_printk(ShowBuf);
                sle_server_send_report_by_handle((uint8_t *)ShowBuf, strlen(ShowBuf) + 1);
                free(ShowBuf);
            }
        }

        osal_msleep(1000);
    }
    return NULL;
}
static void ShipSystem_ControlNode(void)
{
    osal_task *task_handle = NULL;
    osal_task *task_handle1 = NULL;
    osal_task *task_handle2 = NULL;
    osal_kthread_lock();
    // 主任务
    task_handle = osal_kthread_create((osal_kthread_handler)ShipSystem_ControlNode_task, 0, "ShipSystem_ControlNode_task", 0x1000);
    if (task_handle != NULL)
    {
        osal_kthread_set_priority(task_handle, 28);
    }
    // 电量采集任务
    task_handle1 = osal_kthread_create((osal_kthread_handler)Voltage_acquisition_interruption, 0, "Voltage_acquisition_interruption", 0x500);
    if (task_handle1 != NULL)
    {
        osal_kthread_set_priority(task_handle1, 28);
    }
    // 数据上传任务
    task_handle2 = osal_kthread_create((osal_kthread_handler)Control_information_upload, 0, "Control_information_upload", 0x1000);
    if (task_handle2 != NULL)
    {
        osal_kthread_set_priority(task_handle2, 28);
    }

    osal_kthread_unlock();
}

/* Run the ShipSystem_ControlNode. */
app_run(ShipSystem_ControlNode);

// 串口接收数据
void communication_uart_rx_callback(const void *buffer, uint16_t length, bool error)
{
    const unsigned char *data = buffer;
    // 角速度
    ship_attitude.Sx = (data[2] | (data[3] << 8)) / 32767 * 16;
    ship_attitude.Sy = (data[4] | (data[5] << 8)) / 32767 * 16;
    ship_attitude.Sz = (data[6] | (data[7] << 8)) / 32767 * 16;

    // 欧拉角
    ship_attitude.Jx = (data[13] | (data[14] << 8)) / 32768.0 * 180;
    ship_attitude.Jy = (data[15] | (data[16] << 8)) / 32768.0 * 180;
    ship_attitude.Jz = (data[17] | (data[18] << 8)) / 32768.0 * 180;

    ship_attitude.Jz = (int16_t)(ship_attitude.Jz * 10) / 10.0;
    // osal_printk("xyz:%d,%d,%d\n", (int)ship_attitude.Jx, (int)ship_attitude.Jy, (int)ship_attitude.Jz);
}

void PID_out(uint16_t left, uint16_t right)
{

    Motor_Angle.Actual = ship_attitude.Jz;
    PID_Update_Angl(&Motor_Angle);
    Angle = (Motor_Angle.Out / (Motor_Angle.OutMax / 5)) + 16;
    if (Angle <= 11)
        Angle = 10;
    if (Angle >= 21)
        Angle = 21;

    // osal_printk("jd:%d,%d", (int32_t)Motor_Angle.Out, Angle);
    if (direction == 1)
    {
        Motor1_speed.Target = target_speed + Motor_Angle.Out * 2.5;
        Motor2_speed.Target = target_speed - Motor_Angle.Out * 2.5;
    }
    else if (direction == 2)
    {
        Motor1_speed.Target = target_speed + Motor_Angle.Out * 2.5;
        Motor2_speed.Target = target_speed + Motor_Angle.Out * 2.5;
    }
    else if (direction == 3)
    {
        Motor1_speed.Target = target_speed - Motor_Angle.Out * 2.5;
        Motor2_speed.Target = target_speed - Motor_Angle.Out * 2.5;
    }
    else
    {
        Motor1_speed.Target = target_speed - Motor_Angle.Out * 2.5;
        Motor2_speed.Target = target_speed + Motor_Angle.Out * 2.5;
    }

    Motor1_speed.Actual = left;
    PID_Update(&Motor1_speed);

    Motor2_speed.Actual = right;
    PID_Update(&Motor2_speed);
    if (direction == 1)
    {
        Motor1_speed.Out = -Motor1_speed.Out;
        Motor2_speed.Out = -Motor2_speed.Out;
    }
    else if (direction == 2)
    {
        Motor1_speed.Out = -Motor1_speed.Out;
        Motor2_speed.Out = Motor2_speed.Out;
    }
    else if (direction == 3)
    {
        Motor1_speed.Out = Motor1_speed.Out;
        Motor2_speed.Out = -Motor2_speed.Out;
    }

    pwm_test(GPIO_00, PWM_0, Motor1_speed.Out);
    pwm_test(GPIO_04, PWM_4, Motor2_speed.Out);
}
